#include "connect.h"
#include "stdio.h"
#include "string.h"
#include "point.h"

uint8_t rx_buffer[RX_BUFFER_SIZE];
char latest_packet[RX_BUFFER_SIZE];
uint8_t rx_index = 0;
uint8_t rx_data;
uint8_t rx_complete_flag = 0;
uint8_t receiving = 0;
int r_flag = 0;
int g_flag = 0;
int white_piece_flag = 1;
int black_piece_flag = 1;
int piece_board_flag = 1;

static UART_HandleTypeDef *uart_parser_handle = NULL;
static volatile uint8_t tx_busy = 0; 

void UART_Parser_Init(UART_HandleTypeDef *huart)
{
    uart_parser_handle = huart;
    HAL_UART_Receive_IT(uart_parser_handle, &rx_data, 1);
}


void UART_Parser_RxCallback(UART_HandleTypeDef *huart)
{
    if (huart != uart_parser_handle) return;

    if (rx_data == '$') {
        rx_index = 0;
        receiving = 1;
    }
    else if (rx_data == '#') {
        if (receiving) {
            rx_buffer[rx_index] = '\0';
            strcpy((char*)latest_packet, (char*)rx_buffer);  
            rx_complete_flag = 1;
            receiving = 0;
            rx_index = 0;
			memset(rx_buffer, 0, sizeof(rx_buffer));
        }
    }
    else if (receiving) {
        if (rx_index < RX_BUFFER_SIZE - 1) {
            rx_buffer[rx_index++] = rx_data;
        } else {
            receiving = 0;
            rx_index = 0;
        }
    }

    HAL_UART_Receive_IT(uart_parser_handle, &rx_data, 1);
}


void UART_ParseBuffer()
{
    if (rx_complete_flag)
    {
		if (strncmp(latest_packet, "P:", 2) == 0)
        {
            int x1, y1, x2, y2, x3, y3, x4, y4;

            if (sscanf(latest_packet, "P:%d,%d,%d,%d,%d,%d,%d,%d",
                       &x1, &y1, &x2, &y2, &x3, &y3, &x4, &y4) == 8)
            {
                Corners[0].x = x1;  Corners[0].y = y1;  Corners[0].type = EMPTY;
                Corners[1].x = x2;  Corners[1].y = y2;  Corners[1].type = EMPTY;
                Corners[2].x = x3;  Corners[2].y = y3;  Corners[2].type = EMPTY;
                Corners[3].x = x4;  Corners[3].y = y4;  Corners[3].type = EMPTY;
            }
        }
		else if (strncmp(latest_packet, "B:", 2) == 0)
		{
			int x[5], y[5];
			if (sscanf(latest_packet, "B:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
				&x[0], &y[0], &x[1], &y[1], &x[2], &y[2], &x[3], &y[3], &x[4], &y[4]) == 10)
			{
				for (int i = 0; i < 5; i++)
				{
					BlackPieceBuffer[i].x = x[i];
					BlackPieceBuffer[i].y = y[i];
				}
				if (black_piece_flag)
				{
					for (int i = 0; i < 5; i++)
					{
						BlackPiece[i].x = x[i];
						BlackPiece[i].y = y[i];
					}
					black_piece_flag = 0;
				}
			}
		}
		else if (strncmp(latest_packet, "W:", 2) == 0)
		{
			int x[5], y[5];
			if (sscanf(latest_packet, "W:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
				&x[0], &y[0], &x[1], &y[1], &x[2], &y[2], &x[3], &y[3], &x[4], &y[4]) == 10)
			{
				for (int i = 0; i < 5; i++)
				{
					WhitePieceBuffer[i].x = x[i];
					WhitePieceBuffer[i].y = y[i];
				}
				if (white_piece_flag)
				{
					for (int i = 0; i < 5; i++)
					{
						WhitePiece[i].x = x[i];
						WhitePiece[i].y = y[i];
					}
					white_piece_flag = 0;
				}
			}
		}
		else if (strncmp(latest_packet, "N:", 2) == 0)
		{
			int x[9], y[9];
			if (sscanf(latest_packet,
					   "N:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
					   &x[0], &y[0], &x[1], &y[1], &x[2], &y[2],
					   &x[3], &y[3], &x[4], &y[4], &x[5], &y[5],
					   &x[6], &y[6], &x[7], &y[7], &x[8], &y[8]) == 18 && piece_board_flag)
			{
				for (int i = 0; i < 9; i++)
				{
					PieceBoard[i].x = x[i];
					PieceBoard[i].y = y[i];
				}
			}
		}
		else if (strncmp(latest_packet, "M:", 2) == 0)
		{
			int x, y;
			if (sscanf(latest_packet, "M:%d,%d", &x, &y) == 2)
			{
				
				Motor.x = 0.744 * x + 85.736;
				Motor.y = 0.720 * y + 54.685;
				
//				Motor.x = 1.346 * x + -111.182;
//				Motor.y = 1.344 * y + -71.601;
				
//				Motor.x = 0.994 * x + 1.402;
//				Motor.y = 1.019 * y - 7.962;
				
//				Motor.x = 1.2345 * x + (-0.0097) * y + (-65.5662);
//				Motor.y = 0.0005 * x + 1.1871 * y + (-32.7214);
				
//				Motor.x = 0.8574 * x + 77.7980;
//				Motor.y = 0.8819 * y + 138.5467;

				
				//ToptoGround(&Motor);
			}
		}


    }
}



void UART_SendString_IT(UART_HandleTypeDef *huart, const char *str)
{
    if (tx_busy) return;  

    tx_busy = 1;
    HAL_UART_Transmit_IT(huart, (uint8_t *)str, strlen(str));
}


//void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
//{
//    if (huart->Instance == USART1) 
//    {
//        tx_busy = 0; 
//    }
//}

